"""Platooning Manager
"""
import uuid
import random
import traci
from src.simulation_system.utils import compute_distance, compute_gap
from src.sumo_simulation.sumo_manager import SumoSimulation as Sumo


class Platoon:
    """
    Platoon manager. Used to manage all vehicle managers inside the platoon.
    """
    MAX_DISTANCE = 100
    MAX_LENGTH = 8
    # Maneuver states
    IDLE = 0
    GOING_TO_POSITION = 1
    CHECK_LANE = 2
    CLOSING_GAP = 3
    LEAVING = 4
    OPENING_GAP = 5
    WAITING = 6

    tv = 0.6

    def __init__(self, vehicles, cav_world):
        self.pid = str(uuid.uuid1())
        self.cav_world = cav_world
        self._color = (random.randint(0, 255), random.randint(
            0, 255), random.randint(0, 255))
        vehicles.sort(key=lambda v: v.get_lane_pos(), reverse=True)
        vehicles[0].set_color(self._color)
        vehicles[0].set_platoon_id(self.pid)

        lanes = [vehicles[0].get_lane_index()]
        lane_ids = [vehicles[0].get_lane_id()]

        self._topology = dict()
        self._active = True
        self._states = [self.IDLE]
        self._last_time_gap = {}

        if len(vehicles) > 1:
            vehicles[0].set_leader()
        for i in range(1, len(vehicles)):
            vehicles[i].set_member()
            vehicles[i].set_color(self._color)
            vehicles[i].set_platoon_id(self.pid)
            self._topology[vehicles[i].get_vehicle_id()] = {"leader": vehicles[0].get_vehicle_id(),
                                                            "front": vehicles[i - 1].get_vehicle_id()}
            self._states.append(self.WAITING)
            lanes.append(vehicles[i].get_lane_index())
            lane_ids.append(vehicles[i].get_lane_id())

        self._members = vehicles
        self._desired_gap = 7.0
        self._safe_gap = 6
        self.accel = 2.5
        self.decel = -5
        self._merging = False
        self._splitting = False
        self._vehicles_splitting = [False] * len(vehicles)

        leader_max_speed = Sumo.get_vehicle_allowed_speed(self.get_leader().get_vehicle_id())
        lid = self.get_leader().get_lane_id()
        road_max_speed = Sumo.get_lane_max_speed(lid)
        # If the platoon is fast enough merge all vehicles to the leftmost
        # lane, else merge to the rightmost lane
        self._desired_lane = max(lanes) if leader_max_speed > 0.9 * road_max_speed else min(lanes)
        self._desired_lane_id = lane_ids[lanes.index(self._desired_lane)]

        for vehicle in self._members:
            if vehicle.get_lane_index() != self._desired_lane:
                if self.it_is_safe_to_change_lane(vehicle, self._desired_lane_id, self._safe_gap):
                    Sumo.change_lane(vehicle.get_vehicle_id(), self._desired_lane)

        self.nearby_cav = dict()

    def update_info(self):
        if self._active is True:
            self.search()
        if self._active is True:
            self.look_for_splits()
        if self._active is True:
            self.look_for_merge()

    def run_step(self):
        if self._active is True:
            # self.update_desired_speed_and_lane()
            if self._merging:
                self.merge()
            self.communicate()

    def search(self):
        self.nearby_cav = dict()
        for vehicle in self._members:
            vehicle.update_info()
            self.nearby_cav.update(vehicle.get_nearby_cav())
        for vehicle in self._members:
            if vehicle.get_vehicle_id() in self.nearby_cav:
                self.nearby_cav.pop(vehicle.get_vehicle_id())

    def communicate(self):

        front_vehicle = traci.vehicle.getLeader(self.get_leader().get_vehicle_id())
        desired_speed = traci.vehicle.getAllowedSpeed(self._members[0].get_vehicle_id())
        if front_vehicle is None or traci.vehicle.getRouteID(front_vehicle[0]) != traci.vehicle.getRouteID(
                self.get_leader().get_vehicle_id()):
            a = self.get_acc_acceleration(self.get_leader().get_lane_pos(),
                                          self.get_leader().get_lane_pos() + 999,
                                          self.get_leader().get_speed(), 0,
                                          desired_speed)
            traci.vehicle.setAcceleration(self._members[0].get_vehicle_id(), a, 10)
        else:
            a = self.get_acc_acceleration(self.get_leader().get_lane_pos(),
                                          traci.vehicle.getLanePosition(front_vehicle[0]),
                                          self.get_leader().get_speed(), traci.vehicle.getSpeed(front_vehicle[0]),
                                          desired_speed)
            traci.vehicle.setAcceleration(self._members[0].get_vehicle_id(), a, 10)

        # 遍历每辆车辆及其对应的前车和领航车
        for vid, l in self._topology.items():
            # 如果存在前车
            if "front" in l.keys():
                # 获取前车的数据
                speed = traci.vehicle.getSpeed(vid)
                pre_speed = traci.vehicle.getSpeed(l["front"])
                lane_pos = traci.vehicle.getLanePosition(vid)
                pre_lane_pos = traci.vehicle.getLanePosition(l["front"])
                desire_speed = traci.vehicle.getAllowedSpeed(vid)
                a = self.get_cacc_acceleration(lane_pos, pre_lane_pos, speed, pre_speed, desire_speed, vid)

                traci.vehicle.setAcceleration(vid, a, 20)

    def get_acc_acceleration(self, lane_pos, pre_lane_pos, speed, pre_speed, desired_speed):
        distance = (lane_pos - pre_lane_pos) if (lane_pos - pre_lane_pos) > 0 else pre_lane_pos - lane_pos
        if distance > 120:
            a = 0.4 * (desired_speed - speed)
        elif distance < 100:
            a = 0.04 * (distance - self.tv * speed) + 0.8 * (pre_speed - speed)
        else:
            a = 0.23 * (distance - self.tv * speed) + 0.07 * (pre_speed - speed)
        return min(a, self.accel) if a > 0 else max(a, self.decel)

    def get_cacc_acceleration(self, lane_pos, pre_lane_pos, speed, pre_speed, desired_speed, vid):

        distance = pre_lane_pos - lane_pos
        gap_err = distance - speed * self.tv
        # time_gap = distance/speed if speed > 0 else 3
        # speed_err = speed - desired_speed
        # if 1.5 < time_gap < 2:
        #     time_gap = self._last_time_gap[vid]
        # if time_gap > 2:
        #     a = 0.4 * (desired_speed - speed)
        # elif time_gap < 0.2 and speed_err < 0.1:
        #     a = (0.45 * gap_err + 0.25 * (pre_speed - speed)) / (0.1 + 0.25 * self.tv)
        # elif time_gap < 1.5:
        #     a = (0.01*gap_err + 1.6*(pre_speed - speed))/(0.1 + 1.6*self.tv)
        # self._last_time_gap[vid] = time_gap
        # return min(a, self.accel) if a > 0 else max(a, self.decel)
        if speed != 0:
            time_gap = distance / speed
            if time_gap > 2:
                a = 0.4 * (desired_speed - speed)
            else:
                a = (0.45 * gap_err + 0.25 * (pre_speed - speed)) / (0.1 + 0.25 * self.tv)
        else:
            a = (0.45 * gap_err + 0.25 * (pre_speed - speed)) / (0.1 + 0.25 * self.tv)
        return min(a, self.accel) if a > 0 else max(a, self.decel)

    def update_desired_speed_and_lane(self):
        """
        The ACC and CACC of Plexe SUMO seem to limit the desired speed
        automatically to 13.9m/s, so this function should be called at every
        time step to ensure that the maximum speed is updated after every edge
        change. This function also changes the desired lane when a platoon
        composed by one single vehicle has performed a manual lane change
        """

        for vehicle in self._members:
            vid = vehicle.get_vehicle_id()
            lane_max_speed = Sumo.get_vehicle_allowed_speed(vid)
            vehicle_max_speed = Sumo.get_vehicle_max_speed(vid)
            desired_speed = min(lane_max_speed, vehicle_max_speed)
            # Sumo.set_desired_speed(vid, desired_speed)

        if len(self._members) == 1:
            self._desired_lane = self.get_leader().get_lane_index()
            self._desired_lane_id = self.get_leader().get_lane_id()

    def look_for_splits(self):
        if not self.all_members_are_idle():
            for i in range(1, len(self._members)):
                lane_pos = self._members[i].get_lane_pos()
                pre_lane_pos = self._members[i - 1].get_lane_pos()
                for cav_id, pid in self.nearby_cav.items():
                    v_pos = traci.vehicle.getLanePosition(cav_id)
                    if lane_pos < v_pos < pre_lane_pos and traci.vehicle.getLaneIndex(cav_id) == self._desired_lane:
                        self.cav_world.split_platoon(self.pid, i)
                        return

    def look_for_merge(self):
        if self._splitting is not True:
            for cav_id, pid in self.nearby_cav.items():
                if pid is not None:
                    pm = self.cav_world.get_platooning_manager(pid)
                    if self.if_merge(self.nearby_cav, pm):
                        self.cav_world.merge_platoon(self.pid, pid)
                        break

    def get_color(self):
        return self._color

    def get_active(self):
        return self._active

    def get_platoon_len(self):
        return len(self._members)

    def get_leader(self):
        return self._members[0]

    def get_tail(self):
        return self._members[-1]

    def get_member(self, vid):
        for member in self._members:
            if member.get_vehicle_id() == vid:
                return member

    def get_member_index(self, vid):
        for i in range(len(self._members)):
            if self._members[i].get_vehicle_id() == vid:
                return i

    def get_vehicles(self):
        return self._members

    def get_vehicle_ids(self):
        return [v.get_vehicle_id() for v in self._members]

    def get_platoon_id(self):
        return self.pid

    def get_desired_lane(self):
        return self._desired_lane

    def get_desired_lane_id(self):
        return self._desired_lane_id

    def get_edge_list(self):
        return [member.get_road_id() for member in self._members]

    def get_route_list(self):
        return [member.get_route() for member in self._members]

    def get_nearby_cav(self):
        return self.nearby_cav

    def set_active(self, status):
        self._active = status

    def set_merge(self):
        self._merging = True

    def if_splitting(self):
        if self._splitting is True:
            return True
        else:
            return False

    def if_join(self, vehicle):
        if self.MAX_LENGTH and len(self._members) + 1 > self.MAX_LENGTH:
            return False
        self.search()
        if vehicle.get_lane_pos() > self.get_leader().get_lane_pos():
            max_pos = vehicle.get_lane_pos()
            min_pos = self.get_leader().get_lane_pos()
            for vid, pid in self.nearby_cav.items():
                vm = self.cav_world.get_vehicle_manager(vid)
                if min_pos < vm.get_lane_pos() < max_pos:
                    return False
        if vehicle.get_lane_pos() < self.get_tail().get_lane_pos():
            max_pos = self.get_tail().get_lane_pos()
            min_pos = vehicle.get_lane_pos()
            for vid, pid in self.nearby_cav.items():
                vm = self.cav_world.get_vehicle_manager(vid)
                if min_pos < vm.get_lane_pos() < max_pos:
                    return False
        return True

    def if_in_platoon(self, vid):
        for member in self._members:
            if member.get_vehicle_id() == vid:
                return True
        return False

    def if_merge(self, nearby_cav, pm):

        l1 = self.get_platoon_len()
        l2 = pm.get_platoon_len()
        if pm.if_splitting() or l1 + l2 > self.MAX_LENGTH:
            return False

        routes = pm.get_route_list()
        edges_list = pm.get_edge_list()
        neighbor_edges_list = pm.get_edge_list()
        neighbor_routes = pm.get_route_list()
        if not all_edges_in_all_routes(neighbor_edges_list, routes) \
                or not all_edges_in_all_routes(edges_list, neighbor_routes):
            return False

        lane_1 = self.get_desired_lane()
        lane_2 = pm.get_desired_lane()
        lanes = [lane_1, lane_2]
        lane_id_1 = self.get_desired_lane_id()
        lane_id_2 = pm.get_desired_lane_id()
        lane_ids = [lane_id_1, lane_id_2]
        max_speed_1 = Sumo.get_vehicle_max_speed(self.get_leader().get_vehicle_id())
        lid = self.get_leader().get_lane_id()
        road_max_speed = Sumo.get_lane_max_speed(lid)

        desired_lane = max(lanes) if max_speed_1 > 0.9 * road_max_speed else min(lanes)
        desired_lane_id = lane_ids[lanes.index(desired_lane)]

        all_nearby_cav = dict()
        all_nearby_cav.update(nearby_cav)
        all_nearby_cav.update(pm.get_nearby_cav())
        keys_to_remove = []
        for cav_id, pid in list(all_nearby_cav.items()):
            if pid is not None:
                keys_to_remove.append(cav_id)
        for key in keys_to_remove:
            all_nearby_cav.pop(key, None)

        for vid in all_nearby_cav:
            vm = self.cav_world.get_vehicle_manager(vid)
            if vm.get_lane_id() == desired_lane_id:
                max_pos = max(self.get_leader().get_lane_pos(), pm.get_leader().get_lane_pos())
                min_pos = min(self.get_tail().get_lane_pos(), pm.get_tail().get_lane_pos())
                if (min_pos < vm.get_lane_pos() < max_pos and self.if_in_platoon(vid) is False
                        and pm.if_in_platoon(vid) is False):
                    return False
        return True

    def merge(self):
        """
        Executes the merge maneuver finite state machine of every vehicle of
        the platoon
        """
        if self.get_leader().get_lane_index() != self._desired_lane:
            if self.it_is_safe_to_change_lane(self._members[0], self._desired_lane_id, self._safe_gap):
                Sumo.change_lane(self._members[0].get_vehicle_id(), self._desired_lane)

        for i in range(1, len(self._members)):
            lane = self._members[i].get_lane_index()
            lane_front = self._members[i - 1].get_lane_index()

            if self._states[i] is self.WAITING:
                if self._states[i - 1] is not self.WAITING and self._states[i - 1] is not self.GOING_TO_POSITION \
                        or i == 1:
                    self._states[i] = self.GOING_TO_POSITION

            if self._states[i] is self.GOING_TO_POSITION:
                gap = compute_gap(self._members[i].get_lane_pos(), self._members[i - 1].get_lane_pos(), 5)
                if (self._members[i].get_lane_index() == self._desired_lane and self._members[i - 1].get_lane_index()
                        == self._desired_lane):
                    no_need_to_open_gap = True
                else:
                    no_need_to_open_gap = False
                if self._safe_gap < gap and self._states[i - 1] is not self.GOING_TO_POSITION or no_need_to_open_gap:
                    self._states[i] = self.CHECK_LANE
            if self._states[i] is self.CHECK_LANE:
                # # HighLight
                # traci.vehicle.highlight(self._members[i].get_vehicle_id(), (255, 0, 255), alphaMax=1, duration=5)

                if lane == self._desired_lane:
                    self._states[i] = self.CLOSING_GAP
                else:
                    if self.it_is_safe_to_change_lane(self._members[i], self._desired_lane_id, self._safe_gap):
                        Sumo.change_lane(self._members[i].get_vehicle_id(), self._desired_lane)
                        self._states[i] = self.CLOSING_GAP
            if self._states[i] is self.CLOSING_GAP:
                gap = gap_between_vehicles(self._members[i], self._members[i - 1])
                if self._members[i].get_speed() == 0:
                    continue
                time_gap = gap / self._members[i].get_speed()
                if self.tv - 0.3 < time_gap < self.tv + 0.1:
                    self._states[i] = self.IDLE

        if self.all_members_are_idle():
            self._merging = False

    def it_is_safe_to_change_lane(self, vehicle, lane_id, safe_gap):
        """
        Checks whether it is safe or not for a vehicle to change to a lane
        :param vehicle: ID of the vehicle that wants to change lane
        :param lane_id: ID of the destination lane
        :param safe_gap: minimum gap to consider that the lane change is safe
        :type vehicle:
        :type lane_id:
        :type safe_gap: float
        :return: True if it is safe to change lane, False otherwise
        :rtype: bool
        """

        vehicles_in_lane = self.cav_world.get_lane_vehicles(lane_id)
        lane_pos = vehicle.get_lane_pos()
        if lane_pos > 400:
            pass
        for v in vehicles_in_lane:
            v_lane_pos = v.get_lane_pos()
            distance = (lane_pos - v_lane_pos) if (lane_pos - v_lane_pos) > 0 else (v_lane_pos - lane_pos)
            gap = distance - 5
            if gap < safe_gap:
                return False
        return True

    def all_members_are_idle(self):
        for status in self._states:
            if status != self.IDLE:
                return False
        return True


def gap_between_vehicles(vehicle1, vehicle2):
    return compute_distance(vehicle1.get_pos(), vehicle2.get_pos()) - 5


def all_edges_in_all_routes(edges, routes):
    """
    Checks if all the edges contained in the edges list are contained in all
    the routes of the routes list. The routes of the routes list are list of
    edges
    :param edges: list of edges to check
    :param routes: list of routes to check
    :type edges: list[str]
    :type routes: list[list[str]]
    :return: True if all edges of the edges list are contained in the routes
    list
    :rtype: bool
    """
    for route in routes:
        for edge in edges:
            if edge not in route:
                return False

    return True
